/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/


/***********************************************************************
 *
 * FILENAME:
 *  cameraDaemon.cxx
 *
 * PURPOSE:
 *  Provide video positioning system
 *
 * CREATED:
 * 05/2008 by Cory Dixon
 *
 * MODIFIED:
 * 04/2009 by Rob Witoff
 *
 *
 ***********************************************************************/

// includes 
#include <iostream>
#include <iomanip>
#include <errno.h>
#include <mcheck.h>
#include <fcntl.h>
#include <libconfig.h++>
#include <vector>
	
extern "C" {
	#include <libavcodec/avcodec.h>
	#include <libavformat/avformat.h>
	#include <libavdevice/avdevice.h>
	#include <libavutil/log.h>
}       

//#include "signalHandlers.h"
#include "camera.h" 
//#include "virtualCamera.h"     
#include "string.h"	    
//#include "roombaTracker.h"     
//#include "vpsGUI.h"
#include "vcgui3.h"
#include "vcConfig.h"
#include "vps.h"
#include "color.h"
	
using namespace std;

/*<---Global Variables---->*/
/*! \brief determines level of output to user. */
extern bool	 verbose;
extern GuiStruct    guiStr;
/*<-End Global Variables-->*/


// configuration values, set by command line or by the configuration file
//int running	       = 1;
//int debugLevel	    = 0;
//bool showGUI	      = true;
//bool runDaemon	    = false;
bool dumpCap	      = false;
bool saveVideoFiles       = false;
//bool captureBackground    = false;
//bool useBackground	= true;
//bool showFeedback	 = true;
//bool showInterimFeedback  = true;
//bool loadBackground       = true;
//bool saveBackground       = true;
//bool runCalibration       = false;
float runRate	     = 0; // frames per second 0-> as fast as possible
//int ROOMBA_EDGE_THICKNESS = 4;
//int backgroundImgCount = 0;

vector<CamConf> vCamConf;
vector<bool> vSaveVideoInd;
vector<Camera*> vCam;

int mapInd[4] = {-1, -1, -1, -1};
GdkPixbuf *img[4] = {NULL, NULL, NULL, NULL};

// Libconfig - config file parser 
using namespace libconfig;
Config config;
char *sysConfDir[] = { (char*)"", (char*)SYSCONF_DIR, (char*)"./", (char*)"~/", NULL };
string configFile = CONFIG_FILE;
string confFile;

//==================================================
// funcitons definitions
//==================================================
void usage(int argc, char **argv);
void parse_cmdline(int argc, char **argv);
void closeCameras();

//==================================================
// configuration file parsing and saving
//==================================================
void saveConfValues()
{
	cout << "===== Saving configuration values ======" << endl
	     << "\tfile = " << confFile << endl;


	// set the auto convert so that we don't see any dumb conversion exceptions
	config.setAutoConvert(true);

	cout << "\tsaving camera configuration values:" << endl;
	Setting &setting = config.lookup("Cameras");
	for(unsigned int i=0; i<vCam.size(); i++){
		if( vCamConf[i].dev ) {
			int confInd = vCamConf[i].confInd;
			cout << "\t\tcam=" << i << " confInd=" << confInd << " - ";

			setting[confInd]["bin_thresh"] = vCam[i]->bin_thresh;
			setting[confInd]["led_thresh"] = vCam[i]->led_thresh;
			setting[confInd]["dilate"]     = vCam[i]->dilate;

			//Check the type of the camera
			bool isVideo = false;
			bool isCamera = false;
			//if (dynamic_cast<VirtualCamera*>(vCam[i]))
				//isVideo = true;
			//else
				isCamera = true;

			setting[confInd]["isCamera"]   = isCamera;
			setting[confInd]["isVideo"]    = isVideo;

			// to test for NAN, simply see if the number equals itself
			float val;
			val = vCamConf[i].rotation[0];
			if( val == val ) {
				cout << "rotation ";
				for(int j=0; j<3; j++)
					setting[confInd]["rotation"][j] = vCamConf[i].rotation[j];
			}

			val = vCamConf[i].translation[0];
			if( val == val ) {
				cout << "translation ";
				for(int j=0; j<3; j++)
					setting[confInd]["translation"][j] = vCamConf[i].translation[j];
			}

			val = vCamConf[i].distortion[0];
			if( val == val ) {
				cout << "distortion ";
				for(int j=0; j<4; j++)
					setting[confInd]["distortion"][j] = vCamConf[i].distortion[j];
			}

			val = vCamConf[i].intrinsic[0];
			if( val == val ) {
				cout << "intrinsic ";
				for(int j=0; j<9; j++)
					setting[confInd]["intrinsic"][j] = vCamConf[i].intrinsic[j];
			}

			cout << endl;
		}
	}

	config.writeFile( confFile.c_str() );
	cout << endl;
}

bool parseConf()
{
	int confDirInd = 0;
	int confOpen   = 0;

	cout << "==> loading configuration file: ";

	while( !confOpen && sysConfDir[confDirInd] ) {
		confFile = string(sysConfDir[confDirInd]) + configFile;
		try {
			config.readFile( confFile.c_str() );
			confOpen = true;
			cout << confFile << endl;
		}
		catch( FileIOException ) {
			confDirInd++;
		}
		catch( ParseException err ) {
			cout << "-- Error in configuration file --" << endl
				  << "\tfile=" << confFile << endl
				  << "\tline=" << err.getLine() << endl
				  << "\terror=" << err.getError() << endl << endl;
			return false;
		}
	}

	// if no configuration file openened, give some feedback to user
	if( !confOpen ) {
		cout << "-- Configuration file not found, searched for:" << endl;
		confDirInd = 0;
		while( sysConfDir[confDirInd] ) {
			confFile = string(sysConfDir[confDirInd]) + configFile;
			cout << "\t" << confFile << endl;
			confDirInd++;
		}
		cout << endl << endl;
		return false;
	}

	// check version for values and format options supported
	const char *version;
	if( config.lookupValue("Version",version) ) {
		if( string(version) != string(VERSION) ) {
			cout << "-- Configuration file version does not match cameraDaemon version -- " << endl
			     << "\tcameraDaemon=" << VERSION << " config file=" << version << endl << endl;
			return false;
		}

	}

	//------------------------------------------------
	// now parse out all camera options
	//------------------------------------------------

	// PARSE APPLICATION DATA
	try {
		Setting &setting    = config.lookup("Application");
		//runDaemon	   = setting["runDaemon"];
		//showGUI	     = setting["showGUI"];
		//debugLevel	  = setting["debugLevel"];
		saveVideoFiles      = setting["saveVideoFiles"];
		//useBackground       = setting["useBackground"];
		//loadBackground      = setting["loadBackground"];
		//saveBackground      = setting["saveBackground"];
		runRate	     = setting["runRate"];
		//showFeedback	= setting["showFeedback"];
		//showInterimFeedback = setting["showInterimFeedback"];
	}
	catch( SettingTypeException ) {
		cout << "-- Error parsing config file: type of configuration setting incorrect in Application definition"
		     << endl << endl;
		return false;
	}
	catch( SettingNotFoundException ) {
		cout << "-- Error parsing config file: setting not found in Application definition"
		     << endl << endl;
		return false;
	}


	// PARSE CAMERA SETTINGS
	int camInd = 0;
	bool noException = true;

	// see if there is a Cameras definition
	try {
		Setting &setting = config.lookup("Cameras");
		int ind;

		while( noException ) {
			noException = false;
			int id;
			CamConf tempConf;

			// check for ID definition, which is minimally required
			try {
				id = setting[camInd]["id"];
				/*if( id >= NUM_CAMS ) {
					cout << "-- Config file error: camera id=" << id << " too large" << endl << endl;
					return false;
				}*/
				ind = camInd++;
				noException = true;
			}
			catch( SettingTypeException ) {
				cout << "-- Error parsing config file: invalid type of id specified for camera #="
					  << camInd-1 << endl;
				return false;
			}
			catch( SettingNotFoundException ) {
				if( camInd == 0 ) {
					cout << "-- Error parsing config file: No id found in camera=" << camInd << endl;
					return false;
				}
				break;
			}

			// now try for standard configuration params
			try {
				tempConf.useCam     = setting[ind]["use"];
				tempConf.confInd    = ind;
				tempConf.dev	= setting[ind]["dev"];
				tempConf.width      = setting[ind]["width"];
				tempConf.height     = setting[ind]["height"];
				tempConf.bin_thresh = setting[ind]["bin_thresh"];
				tempConf.led_thresh = setting[ind]["led_thresh"];
				tempConf.dilate     = setting[ind]["dilate"];
				tempConf.isCamera   = setting[ind]["isCamera"];
				tempConf.isVideo    = setting[ind]["isVideo"];

				if (setting[ind].exists("doesVideoRepeat"))
					tempConf.doesVideoRepeat =  setting[ind].exists("doesVideoRepeat");
				else
					tempConf.doesVideoRepeat = false;

			} catch( SettingNotFoundException ) {
				cout << "-- Error parsing config file: standard values not defined in camera=" << id << endl << endl;
				return false;
			}

			// finally, the rest checks to see if the camera is or has been calibrated
			try {
				for(int i=0; i<9; i++)
					tempConf.intrinsic[i] = setting[ind]["intrinsic"][i];
			} catch( SettingNotFoundException ) {
					cout << "\tCamera " << id << " intrinsic properties not set" << endl;
					tempConf.intrinsic[0] = NAN;
			}
			try {
				for(int i=0; i<4; i++)
					tempConf.distortion[i] = setting[ind]["distortion"][i];
			} catch( SettingNotFoundException ) {
					cout << "\tCamera " << id << " distortion properties not set" << endl;
					tempConf.distortion[0] = NAN;
			}
			try {
				for(int i=0; i<3; i++)
					tempConf.translation[i] = setting[ind]["translation"][i];
			} catch( SettingNotFoundException ) {
					cout << "\tCamera " << id << " translation properties not set" << endl;
					tempConf.translation[0] = NAN;
			}
			try {
				for(int i=0; i<3; i++)
					tempConf.rotation[i] = setting[ind]["rotation"][i];
			} catch( SettingNotFoundException ) {
					cout << "\tCamera " << id << " rotation properties not set" << endl;
					tempConf.rotation[0] = NAN;
			}

			if ( noException ) //Add Camera to vector if there were no exceptions
			{
				vCamConf.push_back( tempConf );
			}
		}

	}
	catch( SettingTypeException ) {
		cout << "-- Error parsing config file: SettingTypeException in camera definition ind="
			  << camInd-1 << endl;
		return false;
	}
	catch( SettingNotFoundException ) {
		cout << "-- Error parsing config file: no camera definition found" << endl;
		return false;
	}

	cout << endl;
	return true;
}

// close all open camera objects
void closeCameras()
{
	for(uint i=0; i < vCam.size(); i++) {
		if( vCam[i]->useCam ) {
			vCam[i]->stop_capture();
			vCam[i]->close();
		}
	}
	cout << endl;
}

/** ffmpeg_avcodec_log
 *      Handle any logging output from the ffmpeg library avcodec.
 * 
 * Parameters
 *      *ignoreme  A pointer we will ignore
 *      errno_flag The error number value
 *      fmt	Text message to be used for log entry in printf() format.
 *      ap	 List of variables to be used in formatted message text.
 *
 * Returns
 *      Function returns nothing.
 */
void ffmpeg_avcodec_log(void *ignoreme, int errno_flag, const char *fmt, va_list vl)
{
	char buf[1024];

	/* Do not log the message coming from avcodec if the debug_level is not set. */
	//if (debug_level) {

		/* Flatten the message coming in from avcodec */
		vsnprintf(buf, sizeof(buf), fmt, vl);

		printf("!! cameraDaemon -> ffmpeg_avcodec_log (flag=%d) :%s \n", errno_flag, buf);
	//}
}



/************************************************************\
|		  DataToXML_Parser::init		    |
\************************************************************/
/*!
  \brief the initializing function for the XML generating thread.
  \param shm shared memory struct between the gateway threads.
  \pre instance of DataToXML_Parser has been created
  \post the XML utils have been intialized and the inetTxFifo
  and SAVRxFifo have been opened
  \return TRUE on successful completion of initialization, FALSE otherwise
*/
VPS::VPS(String name, int argc, char * argv[]) : ThreadedPipe(name)
{
	cout << GREEN << "---------->>>>>      " YELLOW "VPS" GREEN "      <<<<<----------" << ATTROFF << endl;

	//========== parse conf data =========
	// check for declaration of conf file in command line. if found use it, then run
	// parseConf, and finally run parse_cmd so that the command line arguments overide
	// the values specified in the conf file
	struct stat st;
	if( argc > 1 ) {
		string tmpConf = argv[argc-1];
		if (stat(tmpConf.c_str(), &st) >= 0) {
			configFile = tmpConf;
		}
	}

	// parse command line
	parse_cmdline(argc, argv);

	// parse config file
	if( !parseConf() )
		cout << " !!! ERROR parsing configuration file !!! " << endl;


}

bool VPS::init(ShmStruct * shmStructPtr)
{
	ThreadedPipe::init(shmStructPtr);
	guiStr.shm_ptr = shmStructPtr;

	cout << "Configuring VPS interface : ..................";

	// initialize a gps position for me
	Client * a_client;
	a_client = shmPtr->clients.getClient(0);
	


	//=========== Load Cameras ===============i=
	uint i;
	int nCams = 0;

	for(i=0;i<vCamConf.size();i++)
	{
		if (vCamConf[i].isCamera)
		{
			Camera* tempCam = new Camera(i, vCamConf[i].dev, vCamConf[i].width, vCamConf[i].height);
			tempCam->useCam = vCamConf[i].useCam;

			vCam.push_back(tempCam);
			nCams++;
		}
	       else
		{
			cout << "ERROR: Unrecognized Camera";
			return -1;
		}
	}

	cout << "Cameras Found: " << nCams <<'\n';

	//========= Proceed with Init =========
	int maxFD = -1;

	printf("===============================================================\n");
	printf(" cameraDaemon version: " VERSION " --  built on " __DATE__ " " __TIME__ "\n");
	printf("       openCV version: " CV_VERSION "\n");
	printf("    libavutil version: " AV_STRINGIFY(LIBAVUTIL_VERSION) "\n");
	printf("   libavcodec version: " AV_STRINGIFY(LIBAVCODEC_VERSION) "\n");
	printf("  libavformat version: " AV_STRINGIFY(LIBAVFORMAT_VERSION) "\n");
	printf("  libavdevice version: " AV_STRINGIFY(LIBAVDEVICE_VERSION) "\n");
	#if ENABLE_AVFILTER
	printf("  libavfilter version: " AV_STRINGIFY(LIBAVFILTER_VERSION) "\n");
	#endif
	printf("\n\n");

       //==========================================================
	//  Open and initialize used cameras
	//==========================================================
	for(uint i=0; i < vCam.size(); i++) {
		if( vCam[i]->useCam ) {
			if( vCam[i]->open(vCamConf[i].dev) < 0 ) {
				printf("Camera open error -- exiting\n");
				return EXIT_FAILURE;
			}

			if( vCam[i]->set_size(vCamConf[i].width, vCamConf[i].height) < 0 ) {
				printf("Camera set size error -- exiting\n");
				return EXIT_FAILURE;
			}

			if( vCam[i]->init() < 0 ) {
				printf("Camera init error -- exiting\n");
				return EXIT_FAILURE;
			}

			// setup video file saving
			if( saveVideoFiles ) {
				vCam[i]->saveVideo = true;
				vCam[i]->init_videoFile();
			}

			// setup camera configuration from configuration file
			vCam[i]->bin_thresh = vCamConf[i].bin_thresh;
			vCam[i]->led_thresh = vCamConf[i].led_thresh;
			vCam[i]->dilate     = vCamConf[i].dilate;

			if( vCamConf[i].intrinsic[0] != NAN )
				vCam[i]->setIntrinsic(vCamConf[i].intrinsic);
			if( vCamConf[i].distortion[0] != NAN )
				vCam[i]->setDistortion(vCamConf[i].distortion);
			if( vCamConf[i].translation[0] != NAN )
				vCam[i]->setTranslation(vCamConf[i].translation);
			if( vCamConf[i].rotation[0] != NAN )
				vCam[i]->setRotation(vCamConf[i].rotation);

			// add camera to gui
/*
			if( showGUI ) {
				gui->addCamera(i, vCam[i]->get_dev());
				gui->set_binThresh(i, vCam[i]->bin_thresh);
				gui->set_ledThresh(i, vCam[i]->led_thresh);
				gui->set_dilate(i, vCam[i]->dilate);

				gui->set_kalmanGains(kalmanConf.alpha, kalmanConf.beta, kalmanConf.ki);
				gui->set_Covariances(kalmanConf.Q, kalmanConf.R);
			}
*/

			// this is for the select function
			maxFD = (vCam[i]->get_fd() > maxFD ? vCam[i]->get_fd() : maxFD );

			//cvNamedWindow(vCam[i]->get_dev(), CV_WINDOW_AUTOSIZE);

			printf("OK\n");

		}
	}


	//==========================================================
	// If user selected to dump camera capabilities
	//==========================================================
	if(dumpCap) {
		for(uint i=0; i < vCam.size(); i++) {
			if( vCam[i]->useCam ) {
				vCam[i]->cap_dump();
				//vCam[i]->close();
			}
		}
		return true;
	}

	//==========================================================
	// setup libavcodec libraries.
	//==========================================================
	av_register_all();
	//avcodec_init();
	//avcodec_register_all();

	av_log_set_level( AV_LOG_QUIET );

	//==========================================================
	// start the capture process. the defualt behvideoor of start_capture
	// is to start all frames for each camera, because of this the
	// first num_buffer frames (we use 4) will be out of sync. 
	//==========================================================
	// let the cameras start streaming
	for(uint i=0; i < vCam.size(); i++)
		if( vCam[i]->useCam )
			vCam[i]->start_capture();

	return true;
}

void VPS::exit()
{	
       for(unsigned int i=0; i < vCam.size(); i++) {
		if( vCam[i]->saveVideo) {
			cout << "\tclosing video for camera=" << i << " : ";
			vCam[i]->close_videoFile();
		}
		//cvDestroyWindow(vCam[i]->get_dev());
	}

	cout << "Closing cameras: " << endl;
	closeCameras();


	ThreadedPipe::exit();
}

void VPS::update()
{
	//cout << "VPS::update()" << endl;

	// get the images from the cameras
	for(uint camI=0; camI < vCam.size(); camI++)
	{
		if( vCam[camI]->useCam )
		{
			if (vCam[camI]->read_frame() <= 0) {
				vCam[camI]->useCam = false; //cam is no longer functioning
				cout << "Camera " << camI << " is no longer functioning" << endl;
			}
		}
	}

	
	// trigger the start of another capture, by doing this in its
	// own for loop we maintain the closest synchronization we can
	// between the cameras
	for(uint camI=0; camI < vCam.size(); camI++)
		if( vCam[camI]->useCam )
			vCam[camI]->trigger_capture();

	// display image
	for(uint camI=0; camI < vCam.size(); camI++)
	{
		if( vCam[camI]->useCam )
		{
			vCam[camI]->process_image();

			//IplImage* frame = vCam[camI]->get_bgr_img();
			//cvShowImage(vCam[camI]->get_dev(), frame );

			gdk_threads_enter();

			//if( img[camI] )
				//gdk_pixbuf_unref( img[camI] );

			img[camI] = vCam[camI]->get_pixbuf();
			if( mapInd[camI] < 0 ) {
				mapInd[camI] = guiStr.config.gpsmap->add_videoImage( camI, img[camI])-1;
			} else {
				guiStr.config.gpsmap->update_videoImage(mapInd[camI], img[camI]);
			}
			gdk_threads_leave();

		}
	}
}

VPS::~VPS()
{

}

void parse_cmdline (int argc, char **argv)
{
	// parse command line input ooptions
	int c;
	while ((c=getopt(argc,argv,"d")) != EOF)
		switch( (char)c )
		{
			case 'd':
				dumpCap = true;
				break;

			default:
				;
		}

}

